#ifndef __PLANNER_BRIDGE_H_
#define __PLANNER_BRIDGE_H_

#include "../../../core/core.h"
#include "../../../path_planner/global_planner/global_planner_base.h"
#include "../../../path_planner/global_planner/a_star/a_star.h"
#include "../../../path_planner/local_planner/local_planner_base.h"
#include "../../../path_planner/local_planner/dwa/dwa.h"

class PlannerBridge{
    public:
        PlannerBridge(tf2_ros::Buffer* tf_buffer);
        ~PlannerBridge();

        // 设置地图数据
        void SetMap2D(Map2D& map_2d, boost::shared_ptr<const nav_msgs::OccupancyGrid> map_ptr);
        // 发布速度
        void PublishTwist(Velocity2D cmd_vel);
        // 发布空速
        void PublishTwist();
        // 发布位姿
        void PublishPose(ros::Publisher& pub, Pose2D pose_2d, string frame_id = "map");
        // 发布原始路径
        void PublishPath(ros::Publisher& pub, vector<Pose2D>& points, string frame_id = "map");
        // 发布扩展点
        void PublishExpendPointsMap(vector<Pixel2D>& expend_points);
        // 路径方向计算
        vector<Pose2D> GenPathDirection(vector<Pose2D>& points, Pose2D goal);
        // 生成P2P路径
        bool GenerateP2PPath(Pose2D start, Pose2D goal, vector<Pose2D>& points);
        // 计算速度指令
        bool ComputeTwist(Velocity2D& cmd_vel, LidarScan2D lidar_scan_2d, 
            Pose2D robot_pose, Pose2D goal_pose);
        // 获取机器人当前位姿
        bool GetRobotPose(geometry_msgs::PoseStamped& global_pose); 
        // 获取TF坐标变换
        bool GetTFPose(geometry_msgs::PoseStamped& global_pose, string father_frame, string child_frame,
            ros::Time stamp = ros::Time(0)); 
        // 全局路径转局部路径
        bool PathGlobal2Local(Pose2D robot_pose, vector<Pose2D> global_path, vector<Pose2D>& local_path, float dist_threshold);
        // 检测目标点是否到达 距离和角度
        bool IsGoalReached(Pose2D robot_pose, Pose2D goal_pose, float xy_tolerance, float yaw_tolerance);
        // 检测目标点是否到达  距离判断
        bool IsPositionReached(Pose2D robot_pose, Pose2D goal_pose, float xy_tolerance);
        // 检测目标点是否到达  角度判断
        bool IsAngleReached(Pose2D robot_pose, Pose2D goal_pose, float yaw_tolerance);       
        // 获取离当前位置最近的前瞻点 简单的距离计算
        bool FindLookAheadPointByDistance(vector<Pose2D>& local_path, Pose2D curr, Pose2D& target_point, float lookahead_distance);
        // 旋转到指定角度
        bool RotateToAngle(Velocity2D& cmd_vel, Pose2D curr, Pose2D goal);
        

    private:
        ros::NodeHandle __nh;
        ros::Publisher __path_pub;
        ros::Publisher __path_smooth_pub;
        ros::Publisher __start_pose_pub;
        ros::Publisher __goal_pose_pub;
        ros::Publisher __expand_points_pub;
        ros::Publisher __local_path_pub;
        ros::Publisher __lookahead_pose_pub;
        ros::Publisher __speed_pub;
        ros::Publisher __map_erode_pub;

    private:
        GlobalPlannerType __global_planner_type;    //规划器类型
        boost::shared_ptr<GlobalPlannerBase> __global_planner_ptr;
        LocalPlannerType __local_planner_type;    //规划器类型
        boost::shared_ptr<LocalPlannerBase> __local_planner_ptr;
        boost::shared_ptr<const nav_msgs::OccupancyGrid> __map_ptr;
};

#endif
